//
// Created by erow on 16-3-7.
//
#include <boost/array.hpp>
#include <ros/ros.h>
#include <eigen3/Eigen/Eigen>
#include "sender/motor_data.h"
#include "sender/cmd_hand.h"
#include "sender/cmd_head.h"
#include "sender/cmd_leg.h"
#include "motion/robotModel.h"
#include "std_msgs/Float32.h"

ros::Publisher pub_origin;
ros::Publisher pub_centroid;
robotModel_threePoint robot;
int motor_data[20] = {0};

void leftHandCallback(const sender::cmd_hand &data) {
    //ROS_INFO("leftHandCallback");
    robot.setHand(data.roll, data.pitch, data.length, MODEL::LEFT);
    robot.getData(motor_data);

    sender::motor_data origin_data;
    for (int i = 0; i < 20; i++) {
        origin_data.data[i] = motor_data[i];
        origin_data.map[i] = 1;
    }

    pub_origin.publish(origin_data);

    Eigen::Vector3f v = robot.cal_referCentroid(0);
    std_msgs::Float32 cendroid_x;
    cendroid_x.data = v[1];
    pub_centroid.publish(cendroid_x);
}

void rightHandCallback(const sender::cmd_hand &data) {
    robot.setHand(data.roll, data.pitch, data.length, MODEL::RIGHT);
    robot.getData(motor_data);
    sender::motor_data origin_data;
    for (int i = 0; i < 20; i++) {
        origin_data.data[i] = motor_data[i];
        origin_data.map[i] = 1;
    }

    pub_origin.publish(origin_data);

    Eigen::Vector3f v = robot.cal_referCentroid(0);
    std_msgs::Float32 cendroid_x;
    cendroid_x.data = v[1];
    pub_centroid.publish(cendroid_x);
}

void leftLegCallback(const sender::cmd_leg &data) {
    robot.setLeg(data.roll, data.pitch, data.yaw, data.length, MODEL::LEFT);
    robot.getData(motor_data);
    sender::motor_data origin_data;
    for (int i = 0; i < 20; i++) {
        origin_data.data[i] = motor_data[i];
        origin_data.map[i] = 1;
    }

    pub_origin.publish(origin_data);

    Eigen::Vector3f v = robot.cal_referCentroid(0);
    std_msgs::Float32 cendroid_x;
    cendroid_x.data = v[1];
    pub_centroid.publish(cendroid_x);
}

void rightLegCallback(const sender::cmd_leg &data) {
    robot.setLeg(data.roll, data.pitch, data.yaw, data.length, MODEL::RIGHT);
    robot.getData(motor_data);
    sender::motor_data origin_data;
    for (int i = 0; i < 20; i++) {
        origin_data.data[i] = motor_data[i];
        origin_data.map[i] = 1;
    }

    pub_origin.publish(origin_data);

    Eigen::Vector3f v = robot.cal_referCentroid(0);
    std_msgs::Float32 cendroid_x;
    cendroid_x.data = v[1];
    pub_centroid.publish(cendroid_x);
}

void headCallback(const sender::cmd_head &data) {
    robot.setHead(data.pitch, data.yaw);
    robot.getData(motor_data);
    sender::motor_data origin_data;
    for (int i = 0; i < 20; i++) {
        origin_data.data[i] = motor_data[i];
        origin_data.map[i] = 1;
    }

    pub_origin.publish(origin_data);

    Eigen::Vector3f v = robot.cal_referCentroid(0);
    std_msgs::Float32 cendroid_x;
    cendroid_x.data = v[1];
    pub_centroid.publish(cendroid_x);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "motor_vec_driver");
    ROS_INFO("vector_controller done!");
    ros::NodeHandle n;
    pub_origin = n.advertise<sender::motor_data>("motion/motor", 10);
    ros::Subscriber sub_left = n.subscribe("motion/vec_left_hand", 1, leftHandCallback);

    ros::Subscriber sub_rHand = n.subscribe("motion/vec_right_hand", 1, rightHandCallback);
    ros::Subscriber sub_lLeg = n.subscribe("motion/vec_right_leg", 1, rightLegCallback);
    ros::Subscriber sub_rLeg = n.subscribe("motion/vec_left_leg", 1, leftLegCallback);
    ros::Subscriber sub_head = n.subscribe("motion/vec_head", 1, headCallback);

    pub_centroid = n.advertise<std_msgs::Float32>("motion/centroid_y", 10);
    ros::spin();
}